// (c) Copyright 2013-2014 Stephen Gold <sgold@sonic.net>
// Distributed under the terms of the GNU General Public License

/*
 This file is part of the Bats Game.

 The Bats Game is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by the
 Free Software Foundation, either version 3 of the License, or (at your
 option) any later version.

 The Bats Game is distributed in the hope that it will be useful, but
 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 for more details.

 You should have received a copy of the GNU General Public License
 along with the Bats Game.  If not, see <http://www.gnu.org/licenses/>.
 */
package bats.unit;

import bats.map.MapNode;
import bats.scene.SceneNode;
import com.jme3.bullet.PhysicsSpace;
import com.jme3.bullet.collision.shapes.CollisionShape;
import com.jme3.bullet.objects.PhysicsRigidBody;
import com.jme3.math.FastMath;
import com.jme3.math.Matrix3f;
import com.jme3.math.Vector3f;
import com.jme3.scene.Node;
import com.jme3.scene.Spatial;
import java.util.logging.Level;
import java.util.logging.Logger;
import jme3utilities.MySpatial;
import jme3utilities.math.VectorXZ;
import jme3utilities.physics.OverlapListener;
import jme3utilities.physics.SimpleSolidControl;

/**
 * A simple solid control to walk an oto around on the map until it becomes
 * impaired.
 * <p>
 * Uses dynamic feedback loops to keep each unimpaired oto (more or less)
 * upright and facing in the correct direction with its base supported. The
 * control is based on SimpleSolidControl instead of CharacterControl so the oto
 * reacts naturally to collisions (provided its collision shape is not a mesh).
 * <p>
 * Instructs OtoAnimationControl to play the "walk" animation as appropriate.
 * <p>
 * Guidance uses the MoveControl interface to specify oto movement.
 * <p>
 * Impairment cancels the oto's animations and disables its feedback loops,
 * causing it to topple forward.
 * <p>
 * Each instance is disabled at creation.
 *
 * @author Stephen Gold <sgold@sonic.net>
 */
public class OtoControl
        extends SimpleSolidControl
        implements MoveControl, OverlapListener {
    // *************************************************************************
    // constants

    /**
     * coefficient of friction for the physical object (&ge;0)
     */
    final private static float friction = 0.3f;
    /**
     * maximum turn rate magnitude (radians per second, &ge;0)
     */
    final private static float maxTurnRate = 2f;
    /**
     * minimum advance/back speed to activate the walk animation (in meters per
     * second, &ge;0)
     */
    final private static float minWalkSpeed = 0.4f;
    /**
     * maximum advance speed (in meters per sec, &ge;0) for a standard-sized oto
     */
    final private static float standardMaxAdvanceSpeed = 1.5f;
    /**
     * maximum back up speed (in meters per second, &ge;0) for a standard-sized
     * oto
     */
    final private static float standardMaxBackSpeed = 0.5f;
    /**
     * maximum sidestep speed (in meters per second, &ge;0) for a standard-sized
     * oto
     */
    final private static float standardMaxSideSpeed = 0.5f;
    /**
     * feedback gain to keep the oto supported (per second, &gt;0)
     */
    final private static float supportGain = 30f;
    /**
     * feedback gain to keep the oto facing the correct direction (per second)
     */
    final private static float turnGain = 15f;
    /**
     * message logger for this class
     */
    final private static Logger logger =
            Logger.getLogger(OtoControl.class.getName());
    /**
     * model coordinates of the oto's left foot
     */
    final private static Vector3f leftFootOffset = new Vector3f(1.8f, -5f, 0f);
    /**
     * model cephalic or "up" direction (unit vector)
     */
    final private static Vector3f modelCephalicDirection = Vector3f.UNIT_Y;
    /**
     * model coordinates of the oto's right foot
     */
    final private static Vector3f rightFootOffset =
            new Vector3f(-1.8f, -5f, 0f);
    /**
     * feedback gain to keep the oto upright (per second)
     */
    final private static VectorXZ attitudeGain = new VectorXZ(0f, 30f);
    // *************************************************************************
    // fields
    /**
     * if true, oto collapses; if false, oto maintains itself
     */
    private boolean impaired = false;
    /**
     * how high the oto has raised its feet (in meters)
     */
    private float clearance;
    /**
     * number of destinations received since the previous physics tick (&ge;0)
     */
    private float destinationRequestCount = 0f;
    /**
     * number of guidance direction requests received since the previous physics
     * tick (&ge;0)
     */
    private float directionRequestCount = 0f;
    /**
     * which scene the spatial is in: set by constructor
     */
    final private SceneNode scene;
    /**
     * direction the oto intends to face (unit vector in map coordinates)
     */
    private VectorXZ directionGoal = null;
    /**
     * if destinationRequestCount is positive, the sum of all guidance
     * destinations received since the previous physics tick; otherwise, an
     * averaged destination from a prior physics tick or null if no guidance has
     * been received since newGuidanceInput()
     */
    private VectorXZ requestedDestination = null;
    /**
     * if directionRequestCount is positive, the sum of all guidance directions
     * received since the previous physics tick; otherwise, an averaged
     * direction from a prior physics tick or null if no guidance has been
     * received since newGuidanceIncpu()
     */
    private VectorXZ requestedDirection = null;
    // *************************************************************************
    // constructors

    /**
     * Instantiate a disabled control for an unimpaired oto.
     *
     * @param node which represents the oto (not null)
     * @param scene which contains the spatial (not null)
     * @param direction which the oto will initially face (not null)
     */
    public OtoControl(Node node, SceneNode scene, VectorXZ direction) {
        super(false,
                scene.getWorld().getOtos().createShape(node),
                scene.getWorld().getOtos().calculateMass(node),
                scene.getWorld().getPhysics().getSpace());
        assert mass > 0f : mass;

        this.scene = scene;
        clearance = scene.getWorld().getOtos().getMaxClearance(node);
        directionGoal = direction.clone(); // use spatial.getLocalRotation()?

        assert !isEnabled();
    }
    // *************************************************************************
    // new methods exposed

    /**
     * Create a collision shape for the oto.
     *
     * @return a new instance
     */
    public CollisionShape createShape() {
        Otos otos = scene.getWorld().getOtos();
        CollisionShape shape = otos.createShape(spatial, clearance);
        return shape;
    }

    /**
     * Get the model coordinates of an oto's base.
     *
     * @return a new vector
     */
    public static Vector3f getBase() {
        Vector3f result = leftFootOffset.clone();
        result.addLocal(rightFootOffset);
        result.divideLocal(2f);
        return result;
    }

    /**
     * Impair the oto.
     */
    public void impair() {
        impaired = true;
    }

    /**
     * Test whether the oto is impaired.
     *
     * @return true if impaired, false otherwise
     */
    public boolean isImpaired() {
        return impaired;
    }

    /**
     * Update the oto's collision shape.
     */
    public void updateCollisionShape() {
        CollisionShape shape = createShape();
        changeShape(shape);
    }
    // *************************************************************************
    // MoveControl methods

    /**
     * Get the direction in which the oto wants to face.
     *
     * @return a new unit vector
     */
    @Override
    public VectorXZ copyDirectionGoal() {
        return directionGoal.clone();
    }

    /**
     * Forget all past guidance.
     */
    @Override
    public void newGuidanceInput() {
        destinationRequestCount = 0;
        requestedDestination = null;
        directionRequestCount = 0;
        requestedDirection = null;
    }

    /**
     * Accept destination guidance.
     *
     * @param destination desired map location (not null)
     */
    @Override
    public void requestDestination(VectorXZ destination) {
        logger.log(Level.INFO, "destination={0}", destination);

        if (destinationRequestCount == 0f) {
            requestedDestination = new VectorXZ();
        }
        requestedDestination = requestedDestination.add(destination);
        destinationRequestCount++;
    }

    /**
     * Accept horizontal movement guidance relative to the oto.
     *
     * @param forward desired forward movement (in meters)
     * @param rightward desired rightward movement (in meters)
     */
    @Override
    public void requestMove(float forward, float rightward) {
        VectorXZ localOffset = new VectorXZ(forward, rightward);
        Otos otos = scene.getWorld().getOtos();
        VectorXZ direction = otos.getMapPelvicDirection(spatial);
        VectorXZ mapOffset = localOffset.rotate(direction);
        requestOffset(mapOffset);
    }

    /**
     * Accept horizontal movement guidance relative to the oto.
     *
     * @param offsetMeters desired map offset (in meters, not null)
     */
    @Override
    public void requestOffset(VectorXZ offsetMeters) {
        assert offsetMeters != null;

        float scaleFactor = scene.getScaleFactor();
        VectorXZ offset = offsetMeters.divide(scaleFactor);
        VectorXZ mapLocation = MySpatial.getMapLocation(spatial);
        VectorXZ destination = mapLocation.add(offset);
        requestDestination(destination);
    }

    /**
     * Accept turning guidance.
     *
     * @param turn desired turn relative to the current direction goal (not
     * null)
     */
    @Override
    public void requestTurn(VectorXZ turn) {
        assert turn != null;

        if (directionRequestCount == 0f) {
            requestedDirection = new VectorXZ();
        }
        requestedDirection = requestedDirection.add(turn);
        directionRequestCount++;
    }
    // *************************************************************************
    // OverlapListener methods

    /**
     * React to an overlapping rigid body.
     *
     * @param overlappingBody the overlapping rigid body (not null)
     * @param overlappingSpatial the spatial of the overlapping rigid body (not
     * null)
     * @param localPoint the location of the overlap (rotated and translated to
     * this control's object, but at world scale)
     */
    @Override
    public void onOverlap(PhysicsRigidBody overlappingBody,
            Spatial overlappingSpatial, Vector3f localPoint) {
        assert overlappingBody != null;
        assert overlappingSpatial != null;
        assert localPoint != null;

        if (!isEnabled() || isImpaired()) {
            return;
        }
        float maxClearance =
                scene.getWorld().getOtos().getMaxClearance(spatial); // meters
        float baseY = getBase().y * MySpatial.getUniformScale(spatial);
        float clearanceNeeded = localPoint.y - baseY; // world units
        clearanceNeeded *= scene.getScaleFactor(); // convert to meters
        if (clearanceNeeded <= maxClearance && clearanceNeeded > clearance) {
            /*
             * The collision point is somewhere on the oto's lower leg,
             * so raise its feet to increase ground clearance.
             */
            assert clearanceNeeded >= 0f : clearanceNeeded;
            clearance = clearanceNeeded;
            updateCollisionShape();
        }
    }
    // *************************************************************************
    // SimpleSolidControl methods

    /**
     * (Re-)Initialize this control each time it is added.
     */
    @Override
    public void onAdd() {
        super.onAdd();

        setFriction(friction);
        /*
         * Initialize the guidance inputs.
         * In the absence of guidance, the oto will stay put.
         */
        newGuidanceInput();
    }

    /**
     * De-initialize this control each time it is removed.
     *
     * Meant to be overridden.
     */
    @Override
    protected void onRemove() {
        /*
         * Before removing this control, cancel any walk animation.
         */
        setWalking(false);

        super.onRemove();
    }

    /**
     * Method invoked before each physics tick to set linear and angular rates
     * and update the animation.
     *
     * @param unused
     * @param simInterval
     */
    @Override
    public void prePhysicsTick(PhysicsSpace unused, float simInterval) {
        if (spatial == null || !isEnabled() || !scene.isEnabled() || impaired) {
            return;
        }
        assert simInterval > 0f : simInterval;
        /*
         * If there have been multiple guidance inputs since the previous tick,
         * average them together.
         */
        if (directionRequestCount > 0f) {
            requestedDirection = requestedDirection.normalize();
            directionRequestCount = 0f;
        }
        if (destinationRequestCount > 1f) {
            requestedDestination =
                    requestedDirection.divide(destinationRequestCount);
            destinationRequestCount = 0f;
        }
        /*
         * Turn the oto per guidance.
         */
        assert directionGoal.isUnitVector() : directionGoal;
        if (requestedDirection != null) {
            float maxTurnAngle = maxTurnRate * simInterval;
            VectorXZ turnDirection = requestedDirection.clone();
            turnDirection = turnDirection.clampDirection(maxTurnAngle);
            directionGoal = directionGoal.rotate(turnDirection);
            directionGoal.normalize(); // to cope with rounding error
        }
        assert directionGoal.isUnitVector() : directionGoal;
        /*
         * Use feedback to maintain an upright attitude.
         */
        Matrix3f orientation = getPhysicsRotationMatrix();
        Vector3f cephalicDirection = orientation.mult(modelCephalicDirection);
        VectorXZ attitude = new VectorXZ(cephalicDirection);
        /* feedback seeks to zero out this attitide vector */
        VectorXZ attitudeControlSignal = attitude.rotate(attitudeGain);
        /*
         * Use feedback to face the correct direction.
         */
        Otos otos = scene.getWorld().getOtos();
        VectorXZ direction = otos.getMapPelvicDirection(spatial);
        float directionError = direction.directionError(directionGoal);
        float turnControlSignal = turnGain * directionError; // clockwise=+
        /*
         * Combine the two orientation control signals.
         */
        attitudeControlSignal.rotate(direction);
        Vector3f angularRates =
                attitudeControlSignal.toVector3f(-turnControlSignal);
        setAngularVelocity(angularRates);
        /*
         * Horizontal control signal: move toward the desired location.
         */
        VectorXZ localVelocity;
        if (requestedDestination == null) {
            localVelocity = new VectorXZ();
        } else {
            VectorXZ mapLocation = MySpatial.getMapLocation(spatial);
            VectorXZ mapOffset = requestedDestination.subtract(mapLocation);
            VectorXZ move = mapOffset.rotate(direction.mirrorZ());
            VectorXZ localVelocityGoal = move.divide(simInterval);
            localVelocity = restrictMove(localVelocityGoal);
            float speed = localVelocity.length(); // world units per second
            if (speed > 0f) {
                float distance = move.length(); // world units
                float estimatedTravelTime = distance / speed;
                if (estimatedTravelTime < simInterval) {
                    /*
                     * Reduce speed to avoid overshooting the destination.
                     */
                    localVelocity = localVelocity.mult(estimatedTravelTime
                            / simInterval);
                }
            }
        }
        float scaleFactor = scene.getScaleFactor();
        boolean isWalking =
                FastMath.abs(localVelocity.getX()) > minWalkSpeed / scaleFactor;
        setWalking(isWalking);
        VectorXZ mapVelocity = localVelocity.rotate(directionGoal);
        /*
         * Vertical control signal: keep at least one of
         * the oto's feet supported.
         */
        float startHeight = otos.getMaxClearance(spatial) + 0.01f; // meters
        MapNode map = scene.getWorld().getMap();
        float verticalVelocity = 0f; // meters per second

        Vector3f leftFoot = spatial.localToWorld(leftFootOffset, null);
        Float leftAboveSupport = map.measureDrop(leftFoot, startHeight);
        if (leftAboveSupport != null) {
            verticalVelocity = -supportGain * leftAboveSupport;
        }
        Vector3f rightFoot = spatial.localToWorld(rightFootOffset, null);
        Float rightAboveSupport = map.measureDrop(rightFoot, startHeight);
        if (rightAboveSupport != null) {
            float signal = -supportGain * rightAboveSupport;
            if (signal > verticalVelocity) {
                verticalVelocity = signal;
            }
        }
        verticalVelocity /= scaleFactor;
        /*
         * Combine the two translation control signals.
         */
        Vector3f linearVelocity = mapVelocity.toVector3f(verticalVelocity);
        setLinearVelocity(linearVelocity);
        /*
         * If the feet are raised, lower them slightly.
         */
        if (clearance > 0f) {
            clearance -= 0.01f;
            if (clearance < 0f) {
                clearance = 0f;
            }
            updateCollisionShape();
        }
    }
    // *************************************************************************
    // private methods

    /**
     * Restrict movement speed in the horizontal plane where +X=forward and
     * +Z=right. Interpolation between the four principal directions is
     * performed using two ellipses, one for the +X half-plane and another for
     * the -X half-plane.
     *
     * @param request (in world units per second, unaffected)
     */
    private VectorXZ restrictMove(VectorXZ request) {
        Otos otos = scene.getWorld().getOtos();
        float relativeSize = otos.getRelativeSize(spatial);
        float scaleFactor = scene.getScaleFactor();
        float maxAdvance = standardMaxAdvanceSpeed * relativeSize / scaleFactor;
        float maxBack = standardMaxBackSpeed * relativeSize / scaleFactor;
        float maxSide = standardMaxSideSpeed * relativeSize / scaleFactor;
        if (request.getX() > 0f) {
            /*
             * more forward than backward
             */
            return request.clampElliptical(maxAdvance, maxSide);
        } else {
            return request.clampElliptical(maxBack, maxSide);
        }
    }

    /**
     * Update the animation control to walk or not walk.
     *
     * @param newState if true, play the walk animation; if false, don't
     */
    private void setWalking(boolean newState) {
        OtoAnimationControl animationControl =
                spatial.getControl(OtoAnimationControl.class);
        if (animationControl != null) {
            animationControl.setWalking(newState);
        }
    }
}
